//SERIAL TALK
#define TERM_CHAR '\n'
#define BUF_LEN   128
int italk;
char incomingChar, buf[BUF_LEN];
char *pch, *cmd[2], *str;
boolean stopped;

void ReadSerial() {
  //clean buffer
  if(buf[0] != '\0'){
    memset(buf,'\0',BUF_LEN); 
    italk = 0;
  }

  while(Serial.available()) {
    incomingChar = (char) Serial.read();
    if(incomingChar != TERM_CHAR && italk != BUF_LEN){
      buf[italk++] = incomingChar;
    } 
    else {
      break;
    }
    delay(1); // wait for another byte
  }
}

void parseCmd() {
  memset(cmd, '\0', sizeof(cmd));
  /*cmd[0]='\0';
   cmd[1]='\0';*/

  //Send(str);
  cmd[0] = strtok_r(str, "=", &str);
  cmd[1] = str;

  //Send(cmd[0]);
  //printf("comando: %s, valore: %d\n",cmd[0], atoi(cmd[1]));
}

void manageCmd(){
  if(strcmp(cmd[0],"p") == 0){
    kP = atof(cmd[1]);
  }
  else if(strcmp(cmd[0],"i") == 0){
    kI = atof(cmd[1]);
  }
  else if(strcmp(cmd[0],"d") == 0){
    kD = atof(cmd[1]);
  }
  else if(strcmp(cmd[0],"s") == 0){
    Setpoint = atof(cmd[1]);
  }
  else if(strcmp(cmd[0],"o") == 0){
    mod_output = atof(cmd[1]);
  }
  /*else if(strcmp(cmd[0],"start") == 0){
   stopped = false;
   #ifdef USESERVO
   myservo.attach(9);
   myservo.write(90);
   Serial.println('START');
   delay(1000);
   #endif
   }
   else if(strcmp(cmd[0],"stop") == 0){
   stopped = true;
   #ifdef USESERVO
   myservo.write(90);
   myservo.detach();
   Serial.println('STOP');
   delay(1000);
   #endif
   }*/
}

void SplitCmd(char * buf) {
  if(buf[0] == '\0'){
    return;
  }

  //memset(str, '\0', sizeof(str));

  while ((str = strtok_r(buf, ";", &buf)) != NULL){
    //Send(str);
    parseCmd();
    //memset(str, '\0', sizeof(str));
    manageCmd();
    //    str = strtok(NULL, ";");
    //  Send(".");
  }
}

